%不看这个，只是测试用！！！！！！！！！！
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


clc;
clear;
t_acc = 1;
t_ed = 3;
pos_st = 0;
pos_ed = -pi/2;
pos_final = @(a)(a*t_acc^2+t_acc*(t_ed-a*t_acc)*a-pos_ed+pos_st);
a_value = fsolve(pos_final,0);



value = zeros(3,length(0:0.01:t_ed));
n = 1;
for i=0:0.01:t_ed
    value(:,n) = position_s_traj_info(a_value,t_acc,t_ed,i,pos_st,pos_ed);
    n = n+1;
end



%轨迹绘图
% t = 0:0.01:t_ed;
% subplot(3,1,1);
% plot(t,value(1,:));
% title("position");
% subplot(3,1,2);
% plot(t,value(2,:));
% title("velocity");
% subplot(3,1,3);
% plot(t,value(3,:));
% title("acc");

